#include<stdio.h>
#include "include/LB_Math.h"
#include "include/Viewer_Matrix4x4.h"
#include<tools_node.h>
#include <subdivision_of_polygon.h>
#define Matrix4x4 Viewer_Matrix4x4_

static inline double *out_product(double *a,double*b)
{
    double* re=(double*)malloc(sizeof(double)*3);
    
    re[0]=a[1]*b[2]-a[2]*b[1];
    re[1]=-a[0]*b[2]+a[2]*b[0];
    re[2]=a[0]*b[1]-a[1]*b[0];
    return re; 
}
static inline int normalize(double *a)
{
    double norm=a[0]*a[0]+a[1]*a[1]+a[2]*a[2];
    if(norm==0||fabs(norm)<1e-11)
    {
       return 0; 
    }
    norm=SAFE_SQRT(norm);
    if(norm==0||fabs(norm)<1e-11)
    {
        return 0;
    } 
    a[0]/=norm;a[1]/=norm;a[2]/=norm;
    return 1;
}
static inline  Matrix4x4* test_subdivision_of_polygon_get_axis(double ** v)
{
    double l1[3]={v[1][0]-v[0][0],v[1][1]-v[0][1],v[1][2]-v[0][2]};
    double l2[3]={v[2][0]-v[1][0],v[2][1]-v[1][1],v[2][2]-v[1][2]};

    double* n=out_product(l1,l2);
    if(!normalize(n)||!normalize(l1))
    {
        free(n);
        return NULL;
    } 
    double * n1=out_product(n,l1);
    Matrix4x4 * mat=(Matrix4x4*)malloc(sizeof(Matrix4x4));
    Matrix4x4_init_double(mat);
    double* data=(double*)(mat->data);
    data[0*4+0]=l1[0];data[0*4+1]=l1[1];data[0*4+2]=l1[2];
    data[1*4+0]=n1[0];data[1*4+1]=n1[1];data[1*4+2]=n1[2];
    data[2*4+0]=n[0];data[2*4+1]=n[1];data[2*4+2]=n[2];
    free(n);free(n1);  

   
    return mat; 

}
static inline double** test_subdivision_of_polygon_project_2dim(double** v,int len,Matrix4x4* mat)
{
    Matrix4x4* mi=mat->inverse(mat);
    if(mi==NULL)
    {
        return NULL;
    }
    double* data=(double*)(mi->data);
    double **re=(double**)malloc(sizeof(double*)*len);
    for(int i=0;i<len;i++)
    {
        re[i]=(double*)malloc(sizeof(double)*2);
    }

    for(int i=0;i<len;i++)
    {
        for(int j=0;j<2;j++)
        {
            re[i][j]=0;
            for(int k=0;k<3;k++)
            {
                re[i][j]+=data[j*4+k]*v[i][k];
            }
        }

    }

    Matrix4x4_free(mi);


    return re;
}




void test()
{
    double vs[6][3]={{0,0,0},{1,1,0},{2,1,0},{2,10,0},{1,1.2,0},{-1,1,0}};
    
    int len=6;
    double** v=(double**)malloc(sizeof(double*)*len);
    
    for(int i=0;i<len;i++)
    {
        v[i]=(double*)malloc(sizeof(double)*3);
        v[i][0]=vs[i][0];v[i][1]=vs[i][1];v[i][2]=vs[i][2];
        printf("%lf %lf %lf\n",v[i][0],v[i][1],v[i][2] );
    }
    int **s=(int**)malloc(sizeof(int*)*(len-2));
    for(int i=0;i<len-2;i++)
    {
        s[i]=(int*)malloc(sizeof(int)*3);
    }

    //*****************************
    Matrix4x4* m=(Matrix4x4*)malloc(sizeof(Matrix4x4));
    Matrix4x4_init_double(m);
    double * data=(double*)(m->data);
    data[0*4+0]=1/sqrt(6);data[0*4+1]=1/sqrt(3);data[0*4+2]=1/sqrt(2);
    data[1*4+0]=2/sqrt(6);data[1*4+1]=-1/sqrt(3);data[1*4+2]=0;
    data[2*4+0]=1/sqrt(6);data[2*4+1]=1/sqrt(3);data[2*4+2]=-1/sqrt(2);
    for(int i=0;i<len;i++)
    {
        double temp[3]={0,0,0};  
        for(int j=0;j<3;j++)
        {
            for(int k=0;k<3;k++)
            {
                temp[j]+=data[j*4+k]*v[i][k];
            }
        }
        v[i][0]=temp[0];v[i][1]=temp[1];v[i][2]=temp[2];
        
    }
//*********************
    subdivision_of_polygon(v,len,s); 

    for(int i=0;i<len-2;i++)
    {

        printf("%d %d %d\n",s[i][0],s[i][1],s[i][2] );
    }
    Matrix4x4_free(m);
    // if(is_subdivision_of_polygon_acute_angle(v,len,0))
    // {
    //     printf("shi\n");
    // }

}
int main()
{
    test(); 
    //test_subdivision_of_polygon(vs,len,s); 




	return 0;
}
